module nade.matrix;

import std.stdio;
import std.math;

import derelict.opengl.gl;

import nade.vector;
import nade.angle;


struct Matrix23 {
	real xx = 1, yx = 0;
	real xy = 0, yy = 1;
	real x = 0, y = 0;

	static Matrix23 translation(const Vector2 delta)
	{
		return Matrix23(1, 0, 0, 1, delta.x, delta.y);
	}
	static Matrix23 rotation(T)(const T angle)
	{
		const s = angle.sin;
		const c = angle.cos;
		return Matrix23(c, s, -s, c, 0, 0);
	}
	static Matrix23 transformation(T)(const Vector2 delta, const T angle)
	{
		const s = angle.sin;
		const c = angle.cos;
		return Matrix23(c, s, -s, c, delta.x, delta.y);
	}

	Matrix23 opAssign(const Matrix23 rhs)
	{
		xx = rhs.xx;	yx = rhs.yx;
		xy = rhs.xy;	yy = rhs.yy;
		x = rhs.x;		y = rhs.y;
		return this;
	}
	Vector2 opMul(const Vector2 rhs) const
	{
		return Vector2(rhs.x*xx + rhs.y*xy + x, rhs.x*yx + rhs.y*yy + y);
	}
	Vector2 opDiv(const Vector2 rhs) const
	{
		const d = rhs - origin;
		return Vector2(axisX.dot(d), axisY.dot(d));
	}
	Vector2 opMod(const Vector2 rhs) const
	{
		return Vector2(rhs.x*xx + rhs.y*xy, rhs.x*yx + rhs.y*yy);
	}
	Matrix23 opMul(const Matrix23 rhs) const
	{
		return Matrix23(
			xx*rhs.xx + yx*rhs.xy, xx*rhs.yx + yx*rhs.yy,
			xy*rhs.xx + yy*rhs.xy, xy*rhs.yx + yy*rhs.yy,
			x*rhs.xx + y*rhs.xy + rhs.x, x*rhs.yx + y*rhs.yy + rhs.y
			);
	}

	Matrix23 dup() const
	{
		return Matrix23(
			xx, yx,
			xy, yy,
			x, y
			);
	}
	Matrix23 inverse() const
	{
		const invDet = 1/(xx*yy - yx*xy);

		return Matrix23(
				xx*invDet, xy*invDet,
				yx*invDet, yy*invDet,
				0, 0
				)*Matrix23.translation(-origin);
	}

	void render() const
	{
		double[16] m = [
			xx,	yx,	0,	0,
			xy,	yy,	0,	0,
			0,	0,	1,	0,
			x,	y,	0,	1,
			];

		glMultMatrixd(m.ptr);
	}

	void render(void delegate() dg) const
	{
		glPushMatrix();
		render();

		dg();

		glPopMatrix();
	}

	Vector2 axisX() const { return Vector2(xx, yx); }
	Vector2 axisY() const { return Vector2(xy, yy); }
	Vector2 origin() const { return Vector2(x, y); }

	static const IDENTITY = Matrix23(1, 0, 0, 1, 0, 0);
}


struct Matrix44 {
	double xx = 1, xy = 0, xz = 0, xw = 0;
	double yx = 0, yy = 1, yz = 0, yw = 0;
	double zx = 0, zy = 0, zz = 1, zw = 0;
	double wx = 0, wy = 0, wz = 0, ww = 1;

	Matrix44 opAssign(const double[] rhs)
	{
		xx = rhs[0]; xy = rhs[1]; xz = rhs[2]; xw = rhs[3];
		yx = rhs[4]; yy = rhs[5]; yz = rhs[6]; yw = rhs[7];
		zx = rhs[8]; zy = rhs[9]; zz = rhs[10]; zw = rhs[11];
		wx = rhs[12]; wy = rhs[13]; wz = rhs[14]; ww = rhs[15];
		return this;
	}
	Matrix44 opAssign(const Matrix44 rhs)
	{
		xx = rhs.xx; xy = rhs.xy; xz = rhs.xz; xw = rhs.xw;
		yx = rhs.yx; yy = rhs.yy; yz = rhs.yz; yw = rhs.yw;
		zx = rhs.zx; zy = rhs.zy; zz = rhs.zz; zw = rhs.zw;
		wx = rhs.wx; wy = rhs.wy; wz = rhs.wz; ww = rhs.ww;
		return this;
	}

	Matrix44 opMul(const Matrix44 rhs) const
	{
		return Matrix44(
					xx*rhs.xx + xy*rhs.yx + xz*rhs.zx + xw*rhs.wx,
					xx*rhs.xy + xy*rhs.yy + xz*rhs.zy + xw*rhs.wy,
					xx*rhs.xz + xy*rhs.yz + xz*rhs.zz + xw*rhs.wz,
					xx*rhs.xw + xy*rhs.yw + xz*rhs.zw + xw*rhs.ww,

					yx*rhs.xx + yy*rhs.yx + yz*rhs.zx + yw*rhs.wx,
					yx*rhs.xy + yy*rhs.yy + yz*rhs.zy + yw*rhs.wy,
					yx*rhs.xz + yy*rhs.yz + yz*rhs.zz + yw*rhs.wz,
					yx*rhs.xw + yy*rhs.yw + yz*rhs.zw + yw*rhs.ww,

					zx*rhs.xx + zy*rhs.yx + zz*rhs.zx + zw*rhs.wx,
					zx*rhs.xy + zy*rhs.yy + zz*rhs.zy + zw*rhs.wy,
					zx*rhs.xz + zy*rhs.yz + zz*rhs.zz + zw*rhs.wz,
					zx*rhs.xw + zy*rhs.yw + zz*rhs.zw + zw*rhs.ww,

					wx*rhs.xx + wy*rhs.yx + wz*rhs.zx + ww*rhs.wx,
					wx*rhs.xy + wy*rhs.yy + wz*rhs.zy + ww*rhs.wy,
					wx*rhs.xz + wy*rhs.yz + wz*rhs.zz + ww*rhs.wz,
					wx*rhs.xw + wy*rhs.yw + wz*rhs.zw + ww*rhs.ww
					);
	}

	Matrix44 dup() const
	{
		return Matrix44(
			xx, xy, xz, xw,
			yx, yy, yz, yw,
			zx, zy, zz, zw,
			wx, wy, wz, ww
			);
	}
	Matrix44 inverse() const
	{
		return Matrix44(
			xx, yx, zx, 0,
			xy, yy, zy, 0,
			xz, yz, zz, 0,
			-(wx*xx + wy*xy + wz*xz),
			-(wx*yx + wy*yy + wz*yz),
			-(wx*zx + wy*zy + wz*zz),
			1
			);
	}
	Matrix44 translated(const Vector3 rhs) const
	{
		return Matrix44(
			xx, xy, xz, xw,
			yx, yy, yz, yw,
			zx, zy, zz, zw,
			wx + rhs.x, wy + rhs.y, wz + rhs.y, ww
			);
	}
	Matrix44 scaled(double rhs) const
	{
		return Matrix44(
			xx*rhs, xy*rhs, xz*rhs, xw,
			yx*rhs, yy*rhs, yz*rhs, yw,
			zx*rhs, zy*rhs, zz*rhs, zw,
			wx, wy, wz, ww
			);
	}

	void render() const
	{
		glMultMatrixd(ptr);
	}

	void render(void delegate() dg) const
	{
		glPushMatrix();
		render();

		dg();

		glPopMatrix();
	}

	Vector3 rotate(const Vector3 rhs)
	{
		return Vector3(
			rhs.x * xx + rhs.y * yx + rhs.z * zx,
			rhs.x * xy + rhs.y * yy + rhs.z * zy,
			rhs.x * xz + rhs.y * yz + rhs.z * zz
			);
	}
	Vector3 unrotate(const Vector3 rhs)
	{
		return Vector3(
			rhs.x*xx + rhs.y*xy + rhs.z*xz,
			rhs.x*yx + rhs.y*yy + rhs.z*yz,
			rhs.x*zx + rhs.y*zy + rhs.z*zz
			);
	}
	Vector3 transform(const Vector3 rhs)
	{
		return rotate(rhs) + origin;
	}
	Vector3 untransform(const Vector3 rhs)
	{
		return unrotate(rhs - origin);
	}

	double* ptr() const { return cast(double*)&xx; }

	Vector3 axisX() const { return Vector3(xx, xy, xz); }
	Vector3 axisX(const Vector3 rhs) { xx = rhs.x; xy = rhs.y; xz = rhs.z; return rhs.dup; }

	Vector3 axisY() const { return Vector3(yx, yy, yz); }
	Vector3 axisY(const Vector3 rhs) { yx = rhs.x; yy = rhs.y; yz = rhs.z; return rhs.dup; }

	Vector3 axisZ() const { return Vector3(zx, zy, zz); }
	Vector3 axisZ(const Vector3 rhs) { zx = rhs.x; zy = rhs.y; zz = rhs.z; return rhs.dup; }

	Vector3 origin() const { return Vector3(wx, wy, wz); }
	Vector3 origin(const Vector3 rhs) { wx = rhs.x; wy = rhs.y; wz = rhs.z; return rhs.dup; }

	static {
		Matrix44 grammSchmidt(const Vector3 position, const Vector3 direction)
		{
			Vector3 ax;
			Vector3 ay = direction.normal;
			Vector3 az;

			if(fabs(ay.y) >= (1 - real.epsilon))
			{
				ax = Vector3.AXIS_X;
				az = ax.cross(ay);
			}
			else
			{
				ax = Vector3(-ay.z, 0, ay.x).normal;
				az = ax.cross(ay);
			}

			return Matrix44(
				ax.x, ax.y, ax.z, 0.0,
				ay.x, ay.y, ay.z, 0.0,
				az.x, az.y, az.z, 0.0,
				position.x, position.y, position.z, 1.0
				);
		}

		Matrix44 translation(const Vector3 origin)
		{
			return Matrix44(
				1, 0, 0, 0,
				0, 1, 0, 0,
				0, 0, 1, 0,
				origin.x, origin.y, origin.z, 1
				);
		}

		Matrix44 rotation(const Radians angle, const Vector3 axis)
		{
			const c = angle.cos;
			const s = angle.sin;
			const cc = 1 - c;

			return Matrix44(
				(cc*axis.x*axis.x) + c, (cc*axis.x*axis.y) + (axis.z*s), (cc*axis.x*axis.z) - (axis.y*s), 0,
				(cc*axis.x*axis.y) - (axis.z*s), (cc*axis.y*axis.y) + c, (cc*axis.z*axis.y) + (axis.x*s), 0,
				(cc*axis.x*axis.z) + (axis.y*s), (cc*axis.y*axis.z) - (axis.x*s), (cc*axis.z*axis.z) + c, 0,
				0, 0, 0, 1
				);
		}
	}

	static const IDENTITY = Matrix44(
								1, 0, 0, 0,
								0, 1, 0, 0,
								0, 0, 1, 0,
								0, 0, 0, 1
								);
}
